#ifndef xyz2rgb_h
#define xyz2rgb_h

#include <ros/ros.h>

#include <Eigen/Core>
#include <Eigen/Dense>
#include <opencv2/core/eigen.hpp>

#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>

#include <pcl/point_types.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/common/transforms.h>

#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/synchronizer.h>
#include <message_filters/time_synchronizer.h>

#include "detector.h"
#include <memory>

// #include "detect_msgs/Box.h"   // 自定义box消息
// #include "detect_msgs/Boxes.h"

typedef pcl::PointXYZI PointT;

class XYZ2RGB : public DetectNode
{
 private:
   float fx_, fy_, cx_, cy_;
   float k1_, k2_, k3_, p1_, p2_;
   float roll_, pitch_, yaw_;
   float x_, y_, z_;
   cv::Mat cameraMatrix_, distCoeffs_;
   Eigen::Isometry3d Tcl_ = Eigen::Isometry3d::Identity();
   Eigen::Isometry3d K_ = Eigen::Isometry3d::Identity();

  message_filters::Subscriber<sensor_msgs::PointCloud2> sub_cloud_; // 消息1的订阅
  message_filters::Subscriber<sensor_msgs::Image> sub_img_; // 消息2的订阅
  typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2, sensor_msgs::Image> MySyncPolicy;
  typedef message_filters::Synchronizer<MySyncPolicy> Sync;
  std::shared_ptr<Sync> sync_;

  // ros相关
  cv::Mat img_;
  ros::NodeHandle n_;
  std::string frame_id_;

  std::string topic_img_;
  std::string topic_img_new_;
  sensor_msgs::ImagePtr msg_img_new_;
  ros::Publisher pub_img_;  // 发布
  
  std::string topic_cloud_, topic_cloud_new_;
  pcl::PointCloud<PointT>::Ptr cloud_, cloud_voxel_;
  float filter_size_; 
  bool is_filter_;
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_rgb_;
  ros::Publisher pub_cloud_;
  sensor_msgs::PointCloud2Ptr msg_cloud_new_;

 public:
   XYZ2RGB(std::string engine_path,float conf_thres, float nms_thresh, ros::NodeHandle n);
   void getParam();
   void callBack(const sensor_msgs::PointCloud2ConstPtr& msg_cloud, const sensor_msgs::ImageConstPtr& msg_img);
   void filterVoxel(const pcl::PointCloud<PointT>::Ptr& cloud_in, pcl::PointCloud<PointT>::Ptr& cloud_out, float size);
   void Rt2T(Eigen::Isometry3d& T);
   void cloudProcess(const pcl::PointCloud<PointT>::Ptr& cloud_in, pcl::PointCloud<pcl::PointXYZRGB>::Ptr& cloud_out);
   void pointProcess(const pcl::PointCloud<PointT>::Ptr& cloud_in, pcl::PointCloud<pcl::PointXYZRGB>::Ptr& cloud_out);

};








#endif